
#include "mcuhead.h"
#include "swd.h"

const u32 initdata[]={
	0x40021004,0xf0,//SET TS
	0x40021014,0x00120014,//AHB
	0x40021018,0x00004000,//APB
	0x4002101C,0x00000011,//APB
	
//	0x40013000,0x00000307,//SPI CR1
//	0x40013004,0x00001700,//SPI CR2
//	0x40013000,0x00000347,//SPI CR1
	
//	0x48000800,0x0000A97F,//GPIOC
//	0x4800080C,0x00000400,//
//	0x48000820,0x22200000,//
//	0x48000814,BIT4,//
	
	0x48000000,0x0000FFD4,//GPIOA
	0x48000014,0x00000006,
	0x4800000C,0x00000015,
	
//	0x48000400,0x0000F9FF,//GPIOB
//	0x48000414,BIT4,//
//	
	0x48000C00,0x0000E97B,//GPIOD
	0x48000C0C,0x00001404,//pu
	0x48000C20,0x01000010,
	
	//uart
	0x4001380C,32000000/115200,//bps
	0x40013800,0x0000002D,//cr1
	
//	//adc
//	0x40012414,7,
//	0x40012428,0x00000010,
//	0x4001240c,0x00003000,
//	0x40012408,0x00000005,
//	0x40012408,0x00000005,
	
//	//exttim
//	0x40000008,0x0000C000,
//	0x40000000,1,
	
	//basetim
	0x40001028,31700,//psc
	0x40001000,0x00000001,
};



void initmem(){
	u8 i;
	for(i=0;i<sizeof(initdata)/4;i++){
		*((vu32*)initdata[i])=initdata[i+1];
		i++;
	}
}

void SystemInit(){
	initmem();
}
//uart
#if 1
u8 *UARTBUFF;
u16 SendCount=0;
s16 sendtimeout;
s16 recvtimeout;
u8 recvflag=0;
u8 buff[1280];
u16 rpos=0;

void SendNoWait(u8 *p,u16 len){
	UARTBUFF=p;
	SendCount=len;
	sendtimeout=0;
	rpos=0;
}

void UARTRW(s16 dt){
	if(USART1->ISR&USART_ISR_RXNE){
			buff[rpos++]=USART1->RDR;
			rpos&=1023;
			recvtimeout=2;
	}
	else if(recvtimeout>0){
		if((recvtimeout-=dt)<=0)
			recvflag=1;
	}
	sendtimeout-=dt;
	if(sendtimeout<0)sendtimeout=-1;
	
	if(SendCount)
		if((USART1->ISR&USART_ISR_TC)|(sendtimeout<0)){
				sendtimeout=100;
				USART1->TDR=*UARTBUFF++;
				SendCount--;
		}
}
#endif

//swdcmd
#if 1
enum swdcmd
{
	GETSTATUS = 0x01,
	FORCEDEBUG = 0x02,
	APIV1_RESETSYS = 0x03,
	APIV1_READALLREGS = 0x04,
	APIV1_READREG = 0x05,
	APIV1_WRITEREG = 0x06,
	READMEM_32BIT = 0x07,
	WRITEMEM_32BIT = 0x08,
	RUNCORE = 0x09,
	STEPCORE = 0x0a,
	APIV1_SETFP = 0x0b,
	READMEM_8BIT = 0x0c,
	WRITEMEM_8BIT = 0x0d,
	APIV1_CLEARFP = 0x0e,
	APIV1_WRITEDEBUGREG = 0x0f,
	APIV1_SETWATCHPOINT = 0x10,
	ENTER_JTAG_RESET = 0x00,
	ENTER_SWD_NO_RESET = 0xa3,
	ENTER_JTAG_NO_RESET = 0xa4,
	APIV1_ENTER = 0x20,
	EXIT = 0x21,
	READCOREID = 0x22,
	APIV2_ENTER = 0x30,
	APIV2_READ_IDCODES = 0x31,
	APIV2_RESETSYS = 0x32,
	APIV2_READREG = 0x33,
	APIV2_WRITEREG = 0x34,
	APIV2_WRITEDEBUGREG = 0x35,
	APIV2_READDEBUGREG = 0x36,
	APIV2_READALLREGS = 0x3A,
	APIV2_GETLASTRWSTATUS = 0x3B,
	APIV2_DRIVE_NRST = 0x3C,
	APIV2_GETLASTRWSTATUS2 = 0x3E,
	APIV2_START_TRACE_RX = 0x40,
	APIV2_STOP_TRACE_RX = 0x41,
	APIV2_GET_TRACE_NB = 0x42,
	APIV2_SWD_SET_FREQ = 0x43,
	APIV2_JTAG_SET_FREQ = 0x44,
	APIV2_READ_DAP_REG = 0x45,
	APIV2_WRITE_DAP_REG = 0x46,
	APIV2_READMEM_16BIT = 0x47,
	APIV2_WRITEMEM_16BIT = 0x48,
	APIV2_INIT_AP = 0x4B,
	APIV2_CLOSE_AP_DBG = 0x4C,
};

u8 *lastsend;
u16 lastlen;
u8 lastcount;


u32 r4(u8** i){
	u32 v=0;
	u8* p=*i;
	_32T8LL(v)=*p++;
	_32T8L(v)=*p++;
	_32T8H(v)=*p++;
	_32T8HH(v)=*p++;
	*i=p;
	return v;
}

u32 r2(u8** i){
	u32 v=0;
	u8* p=*i;
	_16T8L(v)=*p++;
	_16T8H(v)=*p++;
	*i=p;
	return v;
}

u8 *a4(u8 *p,u8 addr,u16 len){
	u8 *xp=p+(addr&3);
	while(len--){
		
	}
	return xp;
}



u32 lastwritedata;
void cmd(u8 *output)
{
	static u8 ack;
	static u32 addr;
	u8 i;
	u16 len=0;
	u32 value;
	u8 *p=output+5;
	output+=16;
	switch (*p++)
	{
	case GETSTATUS:
		break;
	case FORCEDEBUG:
		break;
	case APIV1_RESETSYS:
		break;
	case APIV1_READALLREGS:
		break;
	case APIV1_READREG:
		break;
	case APIV1_WRITEREG:
		break;
	case READMEM_32BIT:
//		addr=r4(&p);
//		len=r2(&p);
//		p=(u8*)(((u32)p+4)&-4);
//		SWD.SetDataLenght(2);
//		ack=SWD.ReadMem(addr,len,(u32*)p);
//		output=p;
		
		SWD.SetDataLenght(2);
		addr=r4(&p);
		len=r2(&p);
		p=(u8*)(((u32)p+4)&-4);
		ack=SWD.ReadMem(addr,len,(u32*)p);
		output=p+(addr&3);
		
	break;
	case WRITEMEM_32BIT:
//		addr=r4(&p);
//		len=r2(&p);
//		lastwritedata=*(u32*)output;
//		SWD.SetDataLenght(2);
//		ack=SWD.WriteMem(addr,len,(u32*)p);
//		((u32*)output)[0]=0x80;
//		len=1;
		SWD.SetDataLenght(2);
		addr=r4(&p);
		len=r2(&p);
		if(len>200){
			lastwritedata=255;
		}
		lastwritedata=*(u32*)output;
		ack=SWD.WriteMem(addr,len,(u32*)p);
		((u32*)output)[0]=0x80;
		len=1;
		break;
	case RUNCORE:
		break;
	case STEPCORE:
		break;
	case APIV1_SETFP:
		break;
	case READMEM_8BIT:
		//
		//SWD.SetDataLenght(0);
		//ack=SWD.ReadMem(addr,len,(u32*)p);
		SWD.SetDataLenght(0);
		addr=r4(&p);
		len=r2(&p);
		p=(u8*)(((u32)p+4)&-4);
		ack=SWD.ReadMem(addr,len,(u32*)p);
		output=p+(addr&3);
		break;
	case WRITEMEM_8BIT:
		//
		//SWD.SetDataLenght(0);
		//ack=SWD.WriteMem(addr,len,(u32*)p);
		SWD.SetDataLenght(0);
		addr=r4(&p);
		len=r2(&p);
		lastwritedata=*(u32*)output;
		ack=SWD.WriteMem(addr,len,(u32*)p);
		((u32*)output)[0]=0x80;
		len=1;
		break;
	case APIV1_CLEARFP:
		break;
	case APIV1_WRITEDEBUGREG:
		break;
	case APIV1_SETWATCHPOINT:
		break;
	case ENTER_JTAG_RESET:
		break;
	case ENTER_SWD_NO_RESET:
		break;
	case ENTER_JTAG_NO_RESET:
		break;
	case APIV1_ENTER:
		//
		
		break;
	case EXIT:
		//
		break;
	case READCOREID:
		break;
	case APIV2_ENTER:
		((u32*)output)[0]=SWD.Connect()?0x80:0;
		len=2;
		break;
	case APIV2_READ_IDCODES:
		//
		((u32*)output)[0]=0x80;
		((u32*)output)[1]=SWD.IDCores();
		((u32*)output)[2]=0;
		len=12;
		break;
	case APIV2_RESETSYS:
		break;
	case APIV2_READREG:
		addr=*p;
		ack=SWD.GetReg(addr,&value);
		((u32*)output)[0]=0x80;
		((u32*)output)[1]=value;
		len=8;
		break;
	case APIV2_WRITEREG:
		addr=*p++;
		value=r4(&p);
		ack=SWD.SetReg(addr,value);
		((u32*)output)[0]=0x80;
		len=2;
		break;
	case APIV2_WRITEDEBUGREG:
		addr=r4(&p);
		value=r4(&p);
		ack=SWD.WriteData(addr,value);
		((u32*)output)[0]=0x80;
		len=2;
		break;
	case APIV2_READDEBUGREG:
		addr=r4(&p);
		ack=SWD.ReadData(addr,&value);
		((u32*)output)[0]=0x80;
		((u32*)output)[1]=value;
		len=8;
		break;
	case APIV2_READALLREGS:
		((u32*)output)[0]=0x80;
		for(i=0;i<21;i++)
			SWD.GetReg(i,(u32*)&output[4*i+4]);
		len=88;
		break;
	case APIV2_GETLASTRWSTATUS:
		break;
	case APIV2_DRIVE_NRST:
		((u32*)output)[0]=0x80;
		len=2;
		break;
	case APIV2_GETLASTRWSTATUS2:
		//
		((u32*)output)[0]=ack?0x80:0x18;
		((u32*)output)[1]=ack?0:addr;
		len=12;
		break;
	case APIV2_START_TRACE_RX:
		break;
	case APIV2_STOP_TRACE_RX:
		break;
	case APIV2_GET_TRACE_NB:
		break;
	case APIV2_SWD_SET_FREQ:
		((u32*)output)[0]=0x80;
		len=2;
		break;
	case APIV2_JTAG_SET_FREQ:
		break;
	case APIV2_READ_DAP_REG:
		addr=r2(&p);
		addr=r2(&p);
		ack=SWD.ReadDAP(addr,&value);
		((u32*)output)[0]=0x80;
		((u32*)output)[1]=value;
		len=8;
		break;
	case APIV2_WRITE_DAP_REG:
		addr=r2(&p);
		addr=r2(&p);
		value=r4(&p);
		ack=SWD.WriteDAP(addr,value);
		((u32*)output)[0]=0x80;
		len=2;
		break;
	case APIV2_READMEM_16BIT:
		//
		//SWD.SetDataLenght(1);
		//ack=SWD.ReadMem(addr,len,(u32*)p);
	
		SWD.SetDataLenght(1);
		addr=r4(&p);
		len=r2(&p);
		p=(u8*)(((u32)p+4)&-4);
		ack=SWD.ReadMem(addr,len,(u32*)p);
		output=p+(addr&3);
		break;
	case APIV2_WRITEMEM_16BIT:
		//
		//SWD.SetDataLenght(1);
		//ack=SWD.WriteMem(addr,len,(u32*)p);
		SWD.SetDataLenght(1);
		addr=r4(&p);
		len=r2(&p);
		lastwritedata=*(u32*)output;
		ack=SWD.WriteMem(addr,len,(u32*)p);
		((u32*)output)[0]=0x80;
		len=1;
		break;
	case APIV2_INIT_AP:
		//
		((u32*)output)[0]=0x80;
		len=2;
		break;
	case APIV2_CLOSE_AP_DBG:
		//
		((u32*)output)[0]=0x80;
		len=2;
		break;

	default:
		break;
	}
	if(len!=0){
		SendNoWait(lastsend=output,lastlen=len);
	}
}

#endif

// uartdata
#if 0

static u8 step;
static u8 nextid;
static u16 recvlen;
static u32 packetsum;
static u8* packbuff;
static u8 recvPacket;
#define PacketHead 0x55
#define PEnd 0xAA

typedef struct{
	u8 id;
	u8 next;
	u16 len;
	u8 *pdata;
	u8 sum;
}packetDef;

enum PacketStep{
	pHead,
	pID,
	pNextID,
	pLenHb,
	pLenLb,
	pRecvData,
	pSum,
};

packetDef Packet;

void recvbyte(u8 dat)
{
	switch (step)
	{
	case pHead:
		if (dat == PacketHead)
		{
			step = pID;
			packetsum = 0;
		}
		break;
	case pID:
		Packet.id = dat;
		step = pNextID;
		break;
	case pNextID:
		Packet.next = dat;
		step = pLenHb;
		break;
	case pLenHb:
		Packet.len = dat;
		Packet.len <<= 8;
		step = pLenLb;
		break;
	case pLenLb:
		Packet.len |= dat;
		recvlen = Packet.len;
		step = pRecvData;
		break;
	case pRecvData:
		if (recvlen)
		{
			if (--recvlen == 0)
				step = pSum;
			*packbuff++ = dat;
		}
		break;
	case pSum:
		step = pHead;
		Packet.sum = dat;
		recvPacket = 1;
		if (packetsum != Packet.sum)
			recvPacket = 2;
		else if (Packet.next != nextid)
			recvPacket = 3;
		break;
	default:
		step = pHead;
		break;
	}
	packetsum += dat;
}

void SendPacket(packetDef *def){
	u8 *p=packbuff=buff;
	u8 *pd=def->pdata;
	u16 len=def->len;
	u8 sum=PacketHead;
	*p++=PacketHead;
	sum+=(*p++=def->id);
	sum+=(*p++=++def->next);
	sum+=(*p++=def->len>>8);
	sum+=(*p++=def->len&0xff);
	while(len--)
		sum+=(*p++=*pd++);
	*p++=sum;
	SendNoWait(packbuff,6+def->len);
}

void recvpross(){
	u8 *p=buff;
	u8 dat;
	packbuff=buff;
	while(rpos--)
		recvbyte(*p++);
}

void OnRecvPacket(packetDef *def)
{

}
#endif


#define TIMEMS TIM6->CNT



u32 regvalue;
u32 value[2];
void test(){

	SWD.Connect();
	//SWD.SetDataLenght(2);
	//SWD.ReadMem(0x00000000,1024,(u32*)buff);
	//SWD.Connect();
	SWD.Setup();
	SWD.Halt();
//	//SWD.ReadData(0x20000000,&regvalue);
//	//SWD.Go();
	SWD.SetDataLenght(2);
	value[0]=value[1]=0;
	SWD.WriteMem(0x20000000,8,(u32*)&value);
	SWD.SetDataLenght(0);
	value[0]=0x12345678;
	value[1]=0x90ABCDEF;
	SWD.WriteMem(0x20000001,6,(u32*)&value);
	SWD.SetDataLenght(2);
	value[0]=value[1]=0;
	SWD.ReadMem(0x20000000,8,(u32*)&value);
}

u8 debug=0;
int main(){
	u32 tickcount=0;
	s32 time=0;
	s32 ntime;
	s16 dt;
	s16 timeout=0;
	
	*((vu32*)0x40021004)=0;
	while(1){
		dt=0;
		ntime=TIMEMS;
		if(ntime>time)
			dt=ntime-time;
		else
			dt=(ntime+65536)-time;
		time=ntime;
		tickcount+=dt;
		
		UARTRW(dt);
		if(recvflag){
			recvflag=0;
			rpos=0;
			cmd(buff);
		}
		
		if((timeout-=dt)<=0){
			timeout=500;
			//SendNoWait((u8*)"test\r\n",6);
		}
		#if 1
		if(debug){
			debug=0;
			test();
		}
		#endif
	}
}
